#ifndef NAV_PLANNER_TEST_HPP
#define NAV_PLANNER_TEST_HPP

#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"

namespace nav_planner_test
{
    class NavPlannerTest : public nav2_core::GlobalPlanner
    {
    private:

        nav2_util::LifecycleNode::SharedPtr node_;
        std::string name_,global_frame_;
        std::shared_ptr<tf2_ros::Buffer> buffer_;
        nav2_costmap_2d::Costmap2D *costmap_;
        double interpolation_resolution_;
    public:
        NavPlannerTest() = default;
        ~NavPlannerTest() = default;

        void configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;

        void cleanup() override;

        void activate() override;

        void deactivate() override;

        nav_msgs::msg::Path createPlan(
            const geometry_msgs::msg::PoseStamped &start,
            const geometry_msgs::msg::PoseStamped &goal) override;
    };
}

#endif // NAV_PLANNER_TEST_HPP